#include "Motor.h"
#include <Arduino.h>
#include <esp32-hal-ledc.h>


void Motor::initialize(int pin_IN1,int pin_IN2, int pin_pwm,int pwm_channel)
{
	this->pin_IN1 = pin_IN1;
	this->pin_IN2 = pin_IN2;
	this->pin_pwm = pin_pwm;
	this->pwm_channel = pwm_channel;

	pinMode(pin_IN1,OUTPUT);
	pinMode(pin_IN2,OUTPUT);
	digitalWrite(pin_IN1,LOW);
	digitalWrite(pin_IN2,LOW);
	
	ledcSetup(pwm_channel, frequency, resolution_bits);
	ledcAttachPin(pin_pwm, pwm_channel);
}

void Motor::setPwmDuty(float duty)
{
	duty = constrain(inverse * duty, -1, 1);

	if (duty > 0)
	{
		digitalWrite(this->pin_IN1,HIGH);
		digitalWrite(this->pin_IN2,LOW);
		ledcWrite(this->pwm_channel, (int)(255.f * duty));

	}
	else
	{
		digitalWrite(this->pin_IN1,LOW);
		digitalWrite(this->pin_IN2,HIGH);
		ledcWrite(this->pwm_channel, (int)(255.f * -duty));
	}

}


Motor::Motor(int inv): inverse(inv)
{
}

Motor::~Motor()
{
}